#include "pcloud_scans/k_means.h"

#include <string.h>
using namespace std;

int K=30,N,D=2;  //聚类的数目，数据量，数据的维数
double **data;  //存放数据
int *in_cluster;  //标记每个点属于哪个聚类
int *in_cluster_b;  //标记每个点属于哪个聚类
int *last_cluster;  //标记每个点属于哪个聚类
double **cluster_center;  //存放每个聚类的中心点
 

int k_means(int k_means,int d_means,int n_means, double **K_data)
{	
	// ROS_INFO("k_means");
	if(n_means<1)
		return 0;
	K = k_means, N = n_means, D = d_means, data = K_data;
	in_cluster=(int *)malloc(N * sizeof(int));  //每个数据点所属聚类的标志数组
	in_cluster_b=(int *)malloc(N * sizeof(int));  //每个数据点所属聚类的标志数组
	last_cluster=(int *)malloc(N * sizeof(int));  //每个数据点所属聚类的标志数组
	//cout<<"S5"<<endl;
	//memset(in_cluster,0,sizeof(int)*N);
	//memset(in_cluster_b,0,sizeof(int)*N);
	//memset(last_cluster,0,sizeof(int)*N);
	double arr[N]={0};
	//cout<<"S4"<<endl;
	for(int i=0; i<N; i++)
	{
		arr[i] = data[i][0];
	}
	//cout<<"S3"<<endl;
	vector<double> lightValue_k(arr, arr + N);
	K = int(N/4.5);
	if(K<1) return 0;
	//cout<<"S2"<<endl;
	ke_kmeans(lightValue_k, int(N/4.5));

	//cout<<"S1"<<endl;
	//处理角度接近的的聚类中心
	for(int i=0; i<K; i++)//只能处理成对的聚类中心
	{
		double sum1 = 0;
		int count1=0;
		for(int j=0; j<N; j++)
		{
			if(in_cluster[j] == i)
			{
				sum1 = sum1 + data[j][0];
				count1++;
			}
		}
		if(count1<1)
			continue;
		sum1 = sum1/count1;
		if(sum1<352 && sum1>8)
			continue;
		for(int ii=i+1; ii<K; ii++)
		{
			double sum2 = 0;
			int count2=0;
			for(int j=0; j<N; j++)
			{
				if(in_cluster[j] == ii)
				{
					sum2 = sum2 + data[j][0];
					count2++;
				}
			}
			if(count2<1)
				continue;
			sum2 = sum2/count2;
			if(sum1<352 && sum1>8)
				continue;
			if(fabs(sum1 - sum2)<180)//同一个方向
				continue;
			for(int j=0; j<N; j++)
			{
				if(in_cluster[j] == ii)
				{
					in_cluster[j] = i;
				}
			}
		}
	}
	//cout<<"S"<<endl;
	int temp_cluster=0;
	for(int i=0; i<K; i++)
	{
		vector<double> brr;
		vector<int> temp_arr;
		for(int j=0; j<N; j++)
		{
			if(in_cluster[j] == i)
			{
				//cout<<"id "<<i<<"  "<<data[j][0]<<"  "<<data[j][1]<<"  "<<in_cluster[j]<<endl;
				brr.push_back(data[j][1]);
				temp_arr.push_back(j);
				//in_cluster_b[j] = 0;
			}
		}
		int *temp_in_cluster_b;  //标记每个点属于哪个聚类
		temp_in_cluster_b=(int *)malloc((int)(brr.size()) * sizeof(int));  //每个数据点所属聚类的标志数组
		memset(temp_in_cluster_b,0,sizeof(int)*(int)(brr.size()));
		double min = 9999, max = -9999;
		if(brr.size()==0)
			continue;
		for(int j=0; j<brr.size(); j++)
		{
			if(brr[j]>max)
				max = brr[j];
			if(brr[j]<min)
				min = brr[j];
		}
		if(fabs(max - min) >1 && brr.size()>1)//截距差
		{
			int temp_K = (int)(fabs(max - min)/0.8);//聚类数据
			if(temp_K > brr.size())
				temp_K = brr.size();
			if(temp_K > 10)
				temp_K = 10;
			if(temp_K < 2)
				temp_K = 2;
			ke_kmeans_b(brr, temp_K, min, max, temp_in_cluster_b);
			for(int j=0; j<temp_arr.size(); j++)
			{
				in_cluster_b[temp_arr[j]] = temp_in_cluster_b[j];
				//cout<<"K "<<j<<" "<<in_cluster_b[temp_arr[j]]<<endl;
				temp_cluster ++;
			}
			//cout<<""<<endl<<endl;
		}
		else 
		{
			for(int j=0; j<temp_arr.size(); j++)
			{
				in_cluster_b[temp_arr[j]] = 0;
				//cout<<"K "<<j<<" "<<in_cluster_b[temp_arr[j]]<<endl;
				temp_cluster ++;
			}
			//cout<<""<<endl<<endl;
			continue;
		}
	}
	//for(int j=0; j<N; j++)
	//{
	//	cout<<"直线"<<j<<"类别："<<in_cluster_b[j]<<endl;
	//}
	//cout<<"ok"<<endl;
	int temp_a[5000] = {0, 0};
	for(int j=0; j<N; j++)
	{
		//cout<<"直线"<<j<<"类别："<<in_cluster[j]<<" "<<in_cluster_b[j]<<" "<<in_cluster[j]*10+in_cluster_b[j]+1<<endl;/////////////////////////////////////////////////////////
		temp_a[ in_cluster[j]*10+in_cluster_b[j]+1 ] = in_cluster[j]*10+in_cluster_b[j]+1;
	}
//cout<<"ok1"<<endl;
	vector<vector<int> > cluster_sort;
	int temp_T=0;
	for(int j=0; j<5000; j++)
	{
		if(temp_a[j] != 0)
		{
			vector<int> temp;
			//cout<<temp_T<<" "<<temp_a[j]<<endl;
			temp.push_back(temp_T);//有问题
			temp.push_back(temp_a[j]);
			temp_T++;
			cluster_sort.push_back(temp);
			//cout<<cluster_sort[cluster_sort.size()-1][0]<<" "<<cluster_sort[cluster_sort.size()-1][1]<<endl;
		}
	}
	//for(int k=0; k<cluster_sort.size(); k++)//有问题
	//{
	//	cout<<cluster_sort[k][0]<<" "<<cluster_sort[k][1]<<endl;
	//}
//cout<<"ok2"<<endl;
	for(int j=0; j<N; j++)
	{
		int temp1 = in_cluster[j]*10+in_cluster_b[j]+1;
		for(int k=0; k<cluster_sort.size(); k++)
		{
			if(temp1 == cluster_sort[k][1])
				last_cluster[j] = cluster_sort[k][0];
		}
	}

//cout<<"ok3"<<endl;
	//for(int i=0; i<N; i++)
	//{
		//cout<<"last_cluster "<<i<<": "<<last_cluster[i]<<endl;
	//}
	//cout<<"NUM :"<<temp_T<<endl<<endl;
	return temp_T;
}

void k_select(int k_means, double **K_data, int Lines_Num[][2], int Ground_line, vector<vector<Point_xyset> > All_Lines_Points,  Kmeans_class * Last_class)
{
	//cout<<"1"<<endl;
	K = k_means;
	Kmeans_class K_class[K];
	Kmeans_line temp_point;//RESIZE VECTOR
	for(int i=0; i<K; i++)//分析每一个聚类
	{
		K_class[i].detial.resize(Ground_line,vector<Kmeans_line>(0));
		K_class[i].Class = i;
	}

	for(int i=0, k=0; i<Ground_line; i++)
	{
		for(int j=0; j<Lines_Num[i][0];j++)
		{
			//printf("lines[%d]   K %f   b %f    属于类 %d\n",k,data[k][0],data[k][1],last_cluster[k]);/////////////////////////////////////////////////////////
			K_class[last_cluster[k]].Class = last_cluster[k];//线的分类
			K_class[last_cluster[k]].Lines_num ++;		 //该类中线的总数
			//K_class[in_cluster[k]].line_num[i]++;		 //该类中每条雷达线中总共有多少根线段
			temp_point.Id_16lines = i;					 //该条线的线的ID
			temp_point.Id_alllines = k;					 //该条线总数目ID
			temp_point.K_k = data[k][0];
			temp_point.K_b = data[k][1];
			temp_point.Start_point = All_Lines_Points[0][k];//该线段首尾点的信息
			temp_point.End_point = All_Lines_Points[1][k];
			//printf("dat %d\n",i);
			K_class[last_cluster[k]].detial[i].push_back(temp_point);//分别压到8个队列中
			k++;
		}
	}

	for(int i=0; i<K; i++)//分析每一个聚类
	{
//////////////////////////////////////////////////////////////////////横向分析/////////////////////////////////////////////////////////////////////////////////
		//printf("Num_cluster %d\n",K_class[i].Class);/////////////////////////////////////////////////////////
		std::vector<std::vector<Kmeans_line> > temp_detial;
		temp_detial.resize(8,vector<Kmeans_line>(0));
		for(int j=0; j<Ground_line; j++)//   横向分析 ：分析该聚类中的 在八条线上的数目
		{
			//printf("	行列 %d  的线段数： %d\n",j,int(K_class[i].detial[j].size()));/////////////////////////////////////////////////////////
			//如果只有一条直线，直接存储//还是另外处理？？？
			if(K_class[i].detial[j].size()<2) 
			{
				if(K_class[i].detial[j].size()==0)continue;

				Last_class[i].Class = K_class[i].Class;
				Last_class[i].Lines_num ++ ;
				Last_class[i].detial[j].push_back(K_class[i].detial[j][0]);	//该直线上，只有一根线段				
		//printf("kkk\n");	
			}
			else //某条线上涵盖大于二个以上的线段
			{
				//printf("Num_cluster %d\n",K_class[i].detial[j][m].K_b);
				/*float K_b=0,K_k=0;
				for(int m=0; m<K_class[i].detial[j].size(); m++)//计算平均斜率
				{
					K_k = K_k + K_class[i].detial[j][m].K_k;
					K_b = K_b + K_class[i].detial[j][m].K_b;
				}
				K_k = K_k/K_class[i].detial[j].size();
				K_b = K_b/K_class[i].detial[j].size();*/
		//printf("sss\n");	
				double Sort[(K_class[i].detial[j].size() * (K_class[i].detial[j].size()-1)/2)][7];
				int T=0;
				for(int m=0; m<K_class[i].detial[j].size(); m++)//计算收尾斜率
				{
					for(int n=m+1; n<K_class[i].detial[j].size(); n++)
					{
						//线段的首点
						double p_x1 = All_Lines_Points[0][K_class[i].detial[j][m].Id_alllines].x;
						double p_y1 = All_Lines_Points[0][K_class[i].detial[j][m].Id_alllines].y;
						double p_x2 = All_Lines_Points[0][K_class[i].detial[j][n].Id_alllines].x;
						double p_y2 = All_Lines_Points[0][K_class[i].detial[j][n].Id_alllines].y;
						double theta1 = std::atan2(p_x2-p_x1,p_y2-p_y1)*(180/M_PI);
							   if(theta1<0.0) theta1 = theta1+360;
						//printf("dm+dn theta1      %f\n",theta1);
						//线段的尾点
						double p_x11 = All_Lines_Points[1][K_class[i].detial[j][m].Id_alllines].x;
						double p_y11 = All_Lines_Points[1][K_class[i].detial[j][m].Id_alllines].y;
						double p_x22 = All_Lines_Points[1][K_class[i].detial[j][n].Id_alllines].x;
						double p_y22 = All_Lines_Points[1][K_class[i].detial[j][n].Id_alllines].y;
						double theta2 = std::atan2(p_x22-p_x11,p_y22-p_y11)*(180/M_PI);
								if(theta2<0.0) theta2 = theta2+360;
						//前6扫面线中的线段长度较大，认为是有效的直线
						if(	sqrt((p_x1-p_x11)*(p_x1-p_x11) + (p_y1-p_y11)*(p_y1-p_y11)) > 1+j*0.3 && j<6)
						{
							Last_class[i].Class = K_class[i].Class;
							Last_class[i].detial[j].push_back(K_class[i].detial[j][n]);	
						}
						//printf("dm+dn theta2      %f\n",theta2);
						//横向的平均角
						double theta = Angles_PI((theta1 + theta2)/2.0) ;

						//计算首点间直线与第一条直线的角度差
						double deta = Angles_PI(theta - K_class[i].detial[j][m].K_k);//存储角度差
						double mini_deta = ((int)deta % 180) + (deta - (int)deta);
						if(mini_deta>90) mini_deta = 180-mini_deta;
						Sort[T][0] = mini_deta;
						Sort[T][1] = m; 
						Sort[T][3] = All_Lines_Points[0][K_class[i].detial[j][m].Id_alllines].SET; //线段的首点id
						Sort[T][4] = All_Lines_Points[1][K_class[i].detial[j][m].Id_alllines].SET; //线段的尾点id
						//printf("		dm theta      %f\n",Sort[T][0]);

						//计算末尾点间直线与第二条直线的角度差
						deta = Angles_PI(theta - K_class[i].detial[j][n].K_k);//存储角度差
						mini_deta = ((int)deta % 180) + (deta - (int)deta);
						if(mini_deta>90) mini_deta = 180-mini_deta;
						//printf("		dn theta      %f\n",mini_deta);
						Sort[T][0] = Sort[T][0] + mini_deta;
						Sort[T][2] = n; 
						Sort[T][5] = All_Lines_Points[0][K_class[i].detial[j][n].Id_alllines].SET; //线段的首点id
						Sort[T][6] = All_Lines_Points[1][K_class[i].detial[j][n].Id_alllines].SET; //线段的尾点id
						T++;
						//printf("		dm+dn theta      %f\n",Sort[T-1][0]);
						//printf("		ID end-start12   %d    %d    %d    %d\n",(int)Sort[T-1][3],(int)Sort[T-1][4],(int)Sort[T-1][5],(int)Sort[T-1][6]);						

					}
				}
		//printf("sss\n");
				//排序
				sort_K(Sort, K_class[i].detial[j].size() * (K_class[i].detial[j].size()-1)/2);


//1.问题				//二条以上直线中 只有一条有效的直线怎么处理（其他二条无效，或者假有效，只有一条有效）  ???   记录所有的、最后用平均的来选择一条可能可以？？？。
//2.问题				//近似障碍物，线段分为2类，怎么处理

				//有效直线 赋值到新的空间中 ////////////////////////////////////
				for(int m=0; m<K_class[i].detial[j].size() * (K_class[i].detial[j].size()-1)/2; m++)
				{
					if(Sort[m][0]>10)//角度差距较大
					{
						continue;
					}
					else if(Sort[m][0]<10 && m==0) //角度较小的第一个点
					{
						if(fabs(Sort[m][4]-Sort[m][5])>8  ||  fabs(Sort[m][3]-Sort[m][6])<Lines_Num[j][1]-8)//检测端点id的距离 ////如果点之间id（距离）太近，显然二条直线之间夹角很小/怎么处理？？？？
						{	
							//printf("			the dis or angle is so near !!!!! \n");
							Last_class[i].Class = K_class[i].Class;
							Last_class[i].detial[j].push_back(K_class[i].detial[j][Sort[m][1]]);
							Last_class[i].detial[j].push_back(K_class[i].detial[j][Sort[m][2]]);
						}
					}
					else if(Sort[m][0]<3.1 && m>0) //角度差极小  
					{
						Last_class[i].Class = K_class[i].Class;
						Last_class[i].detial[j].push_back(K_class[i].detial[j][Sort[m][1]]);								
						Last_class[i].detial[j].push_back(K_class[i].detial[j][Sort[m][2]]);								
					}			
				}

				//丢弃重复直线
				for(int m=0; m<Last_class[i].detial[j].size(); m++)
				{
					for(int n=m+1; n<Last_class[i].detial[j].size(); n++)
					{
						if(Last_class[i].detial[j][m].Id_alllines == Last_class[i].detial[j][n].Id_alllines)
						{
							Last_class[i].detial.at(j).erase(Last_class[i].detial.at(j).begin() + n);
							m--;
						}
					}
				}
				Last_class[i].Lines_num = Last_class[i].Lines_num + Last_class[i].detial[j].size();


				//无效直线 赋值到新的空间中 ////////////////////////////////////
				for(int m=0; m<K_class[i].detial[j].size(); m++)
				{
					bool temp_flag=0;
					for(int n=0; n<Last_class[i].detial[j].size(); n++)
					{
						if(K_class[i].detial[j][m].Id_alllines == Last_class[i].detial[j][n].Id_alllines)
						{
							temp_flag=1;
						}
					}
					if(0 == temp_flag)
					{					
						temp_detial[j].push_back(K_class[i].detial[j][m]);//丢弃的直线
						//printf("				error.Id_alllines :%d\n",temp_detial[j][temp_detial[j].size()-1].Id_alllines);
					}
				}	
			}
		}
		//printf("sss\n");
		//无效直线 判断 ////////////////////////////////////
		double Ave_xb=0, Ave_xk=0, size_xnum=0;
		for(int j=0; j<Ground_line; j++)//八条线
		{
			size_xnum = size_xnum + Last_class[i].detial[j].size();
			for(int m=0; m<Last_class[i].detial[j].size(); m++)//计算斜率
			{
					Ave_xk = Ave_xk + Last_class[i].detial[j][m].K_k;
					Ave_xb = Ave_xb + Last_class[i].detial[j][m].K_b;
			}
		}
		if(size_xnum > 1)
		{
			Ave_xk = Ave_xk/size_xnum;//该聚类中平均斜率和截距
			Ave_xb = Ave_xb/size_xnum;
		//	printf("	Ave_xk : %f  Ave_xb : %f \n", Ave_xk, Ave_xb);		
			for(int j=0; j<Ground_line; j++)//八条线
			{
//3.问题   正确的一定都是正确的么，需要判断么？？？需要 ，因为存在一根线的情况

				for(int m=0; m<temp_detial[j].size(); m++)//比较大小    二根线及其以上 中出现漏检
 				{
				//	printf("				 	id:%d  k:%f b:%f\n",temp_detial[j][m].Id_alllines,temp_detial[j][m].K_k,temp_detial[j][m].K_b);
				//	printf("				 	id:    k:%f b:%f\n",fabs(temp_detial[j][m].K_k - Ave_xk),fabs(temp_detial[j][m].K_b - Ave_xb));
					if(fabs(temp_detial[j][m].K_k - Ave_xk)< 3.1 && fabs(temp_detial[j][m].K_b - Ave_xb)<6.6/20)
					{
						//printf("	OK !!!! id:%d  k:%f b:%f\n",temp_detial[j][m].Id_alllines,temp_detial[j][m].K_k,temp_detial[j][m].K_b);
						Last_class[i].detial[j].push_back(temp_detial[j][m]);
						temp_detial[j].erase(temp_detial[j].begin() + m);
						Last_class[i].Lines_num = Last_class[i].Lines_num + 1;
					}
				}
			}
		}
		//printf("pppp\n");
//////////////////////////////////////////////////////////////////////横向分析基本结束/////////////////////////////////////////////////////////////////////////////////
/*
		//debug
		printf("	new类别 %d   lines_num %d\n",Last_class[i].Class,Last_class[i].Lines_num);
		for(int sss=0; sss<Ground_line; sss++)//分析该聚类中的 在八条线上的数目
		{
			printf("		new %d 行列的线段数 %d\n",sss,int(Last_class[i].detial[sss].size()));
			for(int ssss=0; ssss<Last_class[i].detial[sss].size(); ssss++)//分析该聚类中的 在八条线上的数目
			{
				printf("			该线段所属线id： %d    Id_alllines: %d\n",Last_class[i].detial[sss][ssss].Id_16lines, Last_class[i].detial[sss][ssss].Id_alllines);
				printf("			new: K_k: %f   K_b: %f    \n",Last_class[i].detial[sss][ssss].K_k, Last_class[i].detial[sss][ssss].K_b);
				printf("			new: Start: %d   End: %d    \n",Last_class[i].detial[sss][ssss].Start_point.SET, Last_class[i].detial[sss][ssss].End_point.SET);
			}
		}
		printf("           \n\n\n\n\n\n");*/
//////////////////////////////////////////////////////////////////////纵向分析/////////////////////////////////////////////////////////////////////////////////
		//如若一个聚类中的线段数小于三，直接丢弃
		if(Last_class[i].Lines_num>0 && Last_class[i].Lines_num<3)
		{
			Last_class[i].Lines_num = 0;
			for(int j=0; j<Ground_line; j++)//该类全部清空
			{
				if(Last_class[i].detial[j].size()>0)
					Last_class[i].detial[j].erase(Last_class[i].detial[j].begin(), Last_class[i].detial[j].end());
			}
			continue;//分析下一个聚类
		}
		//如若一个聚类只属于一条线，直接丢弃？？？ 应该没问题
		else if(Last_class[i].Lines_num>0 && Last_class[i].Lines_num>2)
		{
			bool temp_flag=0;
			for(int j=0; j<Ground_line; j++)
			{
				if(Last_class[i].detial[j].size() == Last_class[i].Lines_num)
				{
					temp_flag=1;
					break;
				}
			}
			if(1 == temp_flag)
			{
				Last_class[i].Lines_num = 0;
				for(int j=0; j<Ground_line; j++)//该类全部清空
				{
					if(Last_class[i].detial[j].size()>0)
						Last_class[i].detial[j].erase(Last_class[i].detial[j].begin(), Last_class[i].detial[j].end());
				}
				continue;
			}
		}
//前提：横向分析保证了每一条线里面的聚类是正确的
//计算二条直线中 线段和线段首尾点连线之间  的夹角   夹角过大？起码不是一类，分成二类     夹角较小？是一类
		double D_angle[Ground_line * (Ground_line-1)/2][4] = {0,0,0,0};
		int temp_sort[Ground_line] = {0,0};
		int T=0, T_error=0;
		for(int m=0; m<Ground_line; m++)
		{
			for(int n=m+1; n<Ground_line; n++)
			{
				if(0 == Last_class[i].detial[m].size() || 0 == Last_class[i].detial[n].size()) 
					continue;
				else
				{
					int TT=0;
					for(int mm=0; mm<Last_class[i].detial[m].size(); mm++)
					{
						for(int nn=0; nn<Last_class[i].detial[n].size(); nn++)
						{
							//线段的首点
							double p_x1 = All_Lines_Points[0][Last_class[i].detial[m][mm].Id_alllines].x;
							double p_y1 = All_Lines_Points[0][Last_class[i].detial[m][mm].Id_alllines].y;
							double p_x2 = All_Lines_Points[0][Last_class[i].detial[n][nn].Id_alllines].x;
							double p_y2 = All_Lines_Points[0][Last_class[i].detial[n][nn].Id_alllines].y;
							double theta1 = std::atan2(p_x2-p_x1,p_y2-p_y1)*(180/M_PI);
								   if(theta1<0.0) theta1 = theta1+360;
							//线段的尾点
							double p_x11 = All_Lines_Points[1][Last_class[i].detial[m][mm].Id_alllines].x;
							double p_y11 = All_Lines_Points[1][Last_class[i].detial[m][mm].Id_alllines].y;
							double p_x22 = All_Lines_Points[1][Last_class[i].detial[n][nn].Id_alllines].x;
							double p_y22 = All_Lines_Points[1][Last_class[i].detial[n][nn].Id_alllines].y;
							double theta2 = std::atan2(p_x22-p_x11,p_y22-p_y11)*(180/M_PI);
								   if(theta2<0.0) theta2 = theta2+360;
							//横向的平均角
							double theta = Angles_PI((theta1 + theta2)/2.0) ;
							//printf("D_angle:  theta   %f  %f  %f\n",theta,Last_class[i].detial[m][mm].K_k,Last_class[i].detial[n][nn].K_k);
							//计算首点间直线与theta角度差
							double deta = Angles_PI(theta - Last_class[i].detial[m][mm].K_k);//存储角度差
							double mini_deta = ((int)deta % 180) + (deta - (int)deta);
							if(mini_deta>90) mini_deta = 180-mini_deta;
							double D_k = mini_deta;
							//printf("	D_angle:  shuzhi   %f\n",mini_deta);

							//计算末尾点间直线与theta角度差
							deta = Angles_PI(theta - Last_class[i].detial[n][nn].K_k);//存储角度差
							mini_deta = ((int)deta % 180) + (deta - (int)deta);
							if(mini_deta>90) mini_deta = 180-mini_deta;
							D_k = D_k + mini_deta;
							D_k = D_k/2.0;
							double D_b = fabs(Last_class[i].detial[m][mm].K_b - Last_class[i].detial[n][nn].K_b);
							//double D_2k = fabs(Last_class[i].detial[m][mm].K_k - Last_class[i].detial[n][nn].K_k);
							D_angle[T][0] = D_angle[T][0] + D_k;//角度的差距
							D_angle[T][1] = D_angle[T][1] + D_b;//截距的差距
							//printf("	D_angle:  shuzhi   %f\n",mini_deta);
							TT++;
							//printf("	D_angle:  TT   %d\n",TT);
							if(D_k<4.1 && D_b<8.8/20)//角度差和截距都很小
							{
								Last_class[i].detial[m][mm].Weit ++;
								Last_class[i].detial[n][nn].Weit ++;
							}
						}				
					}
					D_angle[T][0] = D_angle[T][0]/TT;//均值角度差、、、、前提是线上分类正确		
					D_angle[T][1] = D_angle[T][1]/TT;		
					D_angle[T][2]=m;
					D_angle[T][3]=n;
					if(D_angle[T][0]>4.1 || D_angle[T][1]>8.8/20 )
					{
						temp_sort[(int)D_angle[T][2]]++;
						temp_sort[(int)D_angle[T][3]]++;	
						T_error++;			
					}
					T++;
				}
			//printf("	D_angle:  id m/n: %d  %d  %f  %f\n",(int)D_angle[T-1][2],(int)D_angle[T-1][3],D_angle[T-1][0],D_angle[T-1][1]);
			}
		}
		//找到纵向不合适的数据，丢弃整个一扫描线线上所有的数据
		//printf("	D_angle  T: %d\n",T);
		for(int m=0; m<Ground_line; m++)//分析每一行
		{
			//printf("	D_angle  id: %d num: %d\n",m,temp_sort[m]);
			int temp_int = round_double((1+sqrt(1+8*T))/2.0);
			if(temp_int<6) temp_int = temp_int-1;
			else temp_int = temp_int-1;
			if(temp_sort[m]>=temp_int && temp_sort[m]>0)//投票制  //三条线12分布 00 01  //四条线13分布/22分布 00 01 02/00 01 10 11
			{	
				//可能有问题的线
				for(int n=0; n<Last_class[i].detial[m].size(); n++)//分析该扫描线上每一线段是否有用
				{
					//存储丢弃的数据
					if(Last_class[i].detial[m][n].Weit<2)
					{
						temp_detial[m].push_back(Last_class[i].detial[m][n]);
						Last_class[i].detial.at(m).erase(Last_class[i].detial.at(m).begin()+n);
						Last_class[i].Lines_num --;
						n--;
						//printf("	D_angle  error\n");
					}
				}
			}
		}

		//无效直线 判断 ////////////////////////////////////
		double Ave_yb=0, Ave_yk=0, size_ynum=0;
		for(int j=0; j<Ground_line; j++)//八条线
		{
			size_ynum = size_ynum + Last_class[i].detial[j].size();
			for(int m=0; m<Last_class[i].detial[j].size(); m++)//计算斜率
			{
					Ave_yk = Ave_yk + Last_class[i].detial[j][m].K_k;
					Ave_yb = Ave_yb + Last_class[i].detial[j][m].K_b;
			}
		}
		if(size_ynum >= 1)
		{
			Ave_yk = Ave_yk/size_ynum;//该聚类中平均斜率和截距
			Ave_yb = Ave_yb/size_ynum;
			//printf("	Ave_yk : %f  Ave_yb : %f \n", Ave_yk, Ave_yb);	///////////////////////////////////////////////////////////	
			for(int j=0; j<Ground_line; j++)//八条线
			{
//3.问题   正确的一定都是正确的么，需要判断么？？？需要 ，因为存在一根线的情况

				for(int m=0; m<temp_detial[j].size(); m++)
 				{
					//printf("				 	id:%d  k:%f b:%f\n",temp_detial[j][m].Id_alllines,temp_detial[j][m].K_k,temp_detial[j][m].K_b);
					//printf("				 	id:    k:%f b:%f\n",fabs(temp_detial[j][m].K_k - Ave_yk),fabs(temp_detial[j][m].K_b - Ave_yb));
					if(fabs(temp_detial[j][m].K_k - Ave_yk)< 4.1 && fabs(temp_detial[j][m].K_b - Ave_yb)<8.8/20)
					{
						//printf("	OK2 !!!! id:%d  k:%f b:%f\n",temp_detial[j][m].Id_alllines,temp_detial[j][m].K_k,temp_detial[j][m].K_b);///////////////////////////////////////////////////////////	
						Last_class[i].detial[j].push_back(temp_detial[j][m]);
						Last_class[i].Lines_num ++;
					}
				}
/*//b均值不准确
				for(int m=0; m<Last_class[i].detial[j].size(); m++)//比较大小    一根线中出现  错检检
				{
					if(fabs(Last_class[i].detial[j][m].K_k - Ave_yk)>10 && fabs(Last_class[i].detial[j][m].K_b - Ave_yk)>10)//改成全局变量
					{
						printf("	ERROR !!!! id:%d  k:%f k:%f\n",Last_class[i].detial[j][m].Id_alllines,Last_class[i].detial[j][m].K_k,Last_class[i].detial[j][m].K_b);
						Last_class[i].detial[j].erase(Last_class[i].detial[j].begin() + m);//??????位置对么？？？？明天检查一下
						Last_class[i].Lines_num = Last_class[i].Lines_num - 1;
						m--;
					}
				}*/
			}
		}
		//printf("ttt\n");

	}
}

void ke_kmeans(vector<double> &sample,int k)
{	
	int len = sample.size();
	vector<double> meanValue(k,0);
	//									cout<<"S"<<endl;
	//³õÊŒ»¯ŸÛÀàÖÐÐÄ
	for(int a = 0; a < k; a++)
	{
		meanValue[a] = (360.0/k)*a + 180.0/k;
		//cout<<meanValue[a]<<"  ";/////////////////////////////////////////////////////////
	}
	//cout<<endl;/////////////////////////////////////////////////////////
	while(1)
	{
		vector<vector<double> > C(k,vector<double>(len, 0));  //ÓÃÓÚŽæŽ¢Àà±ð
		vector<double> numC(k,0);
		//ŒÆËãÃ¿žöÑù±ŸÓëŸùÖµµÄŸàÀë
		for(int b = 0; b < len;++b)
		{
			int minDis = fabs(sample[b] - meanValue[0]);
			int minDisIndex = 0;
			for(int j = 1; j < k; ++j)
			{
				double dis = fabs(sample[b]-meanValue[j]);
				if(dis < minDis)
				{
					minDis = dis;
					minDisIndex = j;
				}
			}
			//样本归类
			C[minDisIndex][numC[minDisIndex]] = sample[b];
			numC[minDisIndex]++;
			in_cluster[b] = minDisIndex;  //每个数据点所属聚类的标志数组
		}
		//								cout<<"s2"<<endl;
		//break;
		//ŸùÖµžüÐÂ
		double ifBreak = 0;
		for(int c = 0; c < k; ++c)
		{

			double Sum = 0;
			for(int j = 0; j < numC[c]; ++j)
			{
				Sum += C[c][j];
			}
			double lastMeanValue = meanValue[c];
			if(numC[c] == 0)
				meanValue[c] = 0;
			else 
			{
				meanValue[c] = Sum*0.1/numC[c];//ŒÆËãŸùÖµ
				//cout<<"last "<<lastMeanValue<<endl;
				//cout<<"now  "<<meanValue[c]<<endl;
			}

			if(lastMeanValue - meanValue[c]) //±ŸŽÎŸùÖµºÍÉÏÒ»ŽÎµÄÒ»Ñù
				ifBreak++;
		}

		//ÅÐ¶ÏŸùÖµÊÇ·ñ±»žüÐÂ
		if(ifBreak == k)  
		{
			double maxNum = 0;
			double maxNumIndex = 0;
			for(int i = 0; i < k; ++i)
			{
				if( C[i].size() > maxNum)
				{
					maxNum = numC[i];
					maxNumIndex = i;
				}
			}
			//cout << meanValue[maxNumIndex] << endl;///////////////////////////////////////////////////////////	
			//for (int m = 0; m < k; m++)
			//{
				//cout << m << ":\t";///////////////////////////////////////////////////////////
				//for (int n = 0; n < numC[m]; n++)
						//cout << C[m][n] << "\t";
				//cout << endl;///////////////////////////////////////////////////////////	
			//}
			break;
		}
	}
}
void ke_kmeans_b(vector<double> &sample, int k, double min, double max,int *temp_in_cluster_b)
{	
	int len = sample.size();
	vector<double> meanValue(k,0);
	//cout<<"截距分配："<<" ";/////////////////////////////////////////////////////////
	for(int a = 0; a < k; a++)
	{
		meanValue[a] = min + (max-min)/k*a + (max-min)/k/2.0;
		//cout<<meanValue[a]<<" ";/////////////////////////////////////////////////////////
	}
	//cout<<endl;
	//cout<<"ssss1"<<endl;
	int T = 300;
	while(T>0)
	{
		T--;
		vector<vector<double> > C(k,vector<double>(len, 0));  //ÓÃÓÚŽæŽ¢Àà±ð
		vector<double> numC(k,0);
		//ŒÆËãÃ¿žöÑù±ŸÓëŸùÖµµÄŸàÀë
		for(int b = 0; b < len;++b)
		{
			int minDis = fabs(sample[b] - meanValue[0]);
			int minDisIndex = 0;
			for(int j = 1; j < k; ++j)
			{
				double dis = fabs(sample[b]-meanValue[j]);
				if(dis < minDis)
				{
					minDis = dis;
					minDisIndex = j;
				}
			}
			//Ã¿žöÑù±ŸÊôÓÚÄÄžöÀà
			C[minDisIndex][numC[minDisIndex]] = sample[b];
			numC[minDisIndex]++;
			temp_in_cluster_b[b] = minDisIndex;
		}
		//cout<<"ssss2"<<endl;
		//break;
		//ŸùÖµžüÐÂ
		double ifBreak = 0;
		for(int c = 0; c < k; ++c)
		{

			double Sum = 0;
			for(int j = 0; j < numC[c]; ++j)
			{
				Sum += C[c][j];
			}
			double lastMeanValue = meanValue[c];
			if(numC[c] == 0)
				meanValue[c] = 0;
			else 
			{
				meanValue[c] = Sum*0.1/numC[c];//ŒÆËãŸùÖµ
				//cout<<"last "<<lastMeanValue<<endl;
				//cout<<"now  "<<meanValue[c]<<endl;
			}

			if(lastMeanValue - meanValue[c]) //±ŸŽÎŸùÖµºÍÉÏÒ»ŽÎµÄÒ»Ñù
				ifBreak++;
		}
		//cout<<"ssss3"<<endl;
		//ÅÐ¶ÏŸùÖµÊÇ·ñ±»žüÐÂ
		if(ifBreak == k)  
		{
			double maxNum = 0;
			double maxNumIndex = 0;
			for(int i = 0; i < k; ++i)
			{
				if( C[i].size() > maxNum)
				{
					maxNum = numC[i];
					maxNumIndex = i;
				}
			}
			//cout << meanValue[maxNumIndex] << endl;
			//for (int m = 0; m < k; m++)/////////////////////////////////////////////////////////
			//{
				//cout << m << ":\t";
				//for (int n = 0; n < numC[m]; n++)
						//cout << C[m][n] << "\t";
				//cout << endl;
			//}/////////////////////////////////////////////////////////
			break;
			//cout <<" "<<endl<<endl<<endl;/////////////////////////////////////////////////////////
		}
	}
}
//四舍五入
int round_double(double number)
{
    return (number > 0.0) ? (number + 0.5) : (number - 0.5); 
}
 
//动态创建二维数组
double **K_array(int m,int n)
{
	int i;
	double **p;
	p=(double **)malloc(m*sizeof(double *));
	p[0]=(double *)malloc(m*n*sizeof(double));
	for(i=1;i<m;i++)    p[i]=p[i-1]+n;
	return p;
}
 
//释放二维数组所占用的内存
void freearray(double **p)
{
	free(*p);
	free(p);
}
 

/***********************************************************两个属性同时聚类***************************************************************/
//计算欧几里得距离
double getDistance(double avector[],double bvector[])
{
	double theta = fabs(avector[0] - bvector[0]);//角度差
	double b = fabs(avector[1] - bvector[1]);
	if(theta>180)theta=360-theta;
	double sum = theta*theta + b*b;
	return sqrt(sum);
}
 
//把N个数据点聚类，标出每个点属于哪个聚类
void cluster()
{
	int i,j;
	double min;
	double **distance=K_array(N,K);  //存放每个数据点到每个中心点的距离

	for(i=0;i<N;++i)
	{
		min=9999999.0;
		for(j=0;j<K;++j)
		{
			distance[i][j] = getDistance(data[i],cluster_center[j]);
			//printf("%f\n", distance[i][j]);
			if(distance[i][j]<min)
			{
				min=distance[i][j];
				in_cluster[i]=j;
			}
		}
		//printf("data[%d] 属于类-%d\n",i,in_cluster[i]);
	}
	free(distance);
}
 
//计算所有聚类的中心点与其数据点的距离之和
double getDifference()
{
	int i,j;
	double sum=0.0;
	for(i=0;i<K;++i){
		for(j=0;j<N;++j)
		{
			if(i==in_cluster[j])
				sum+=getDistance(data[j],cluster_center[i]);
		}
	}
	return sum;
}
 
//计算每个聚类的中心点
void getCenter(int in_cluster[])
{
	double **sum=K_array(K,D);  //存放每个聚类中心点
	//float sum[K][D];  //也可使用C99变长数组
	long long int i,j,q,count;
	for(i=0;i<K;i++)
		for(j=0;j<D;j++)
			sum[i][j]=0.0;
	for(i=0;i<K;i++)
	{
		count=0;  //统计属于某个聚类内的所有数据点
		for(j=0;j<N;j++)
		{
			if(i==in_cluster[j])
			{
				for(q=0;q<D;q++)
					sum[i][q]+=data[j][q];  //计算所属聚类的所有数据点的相应维数之和
				count++;
			}
		}
		for(q=0;q<D;q++)
		{
			if(count > 0)//应该取出其他聚类中，距离最远的一个，作为该类别
			{

				cluster_center[i][q]=sum[i][q]/count;
			}
		}

	}
	free(sum);
}



void sort_K(double Array[][7],int num)//排序，由小到大
{    
	double temp_float;
	int temp_int1,temp_int2,temp_int3,temp_int4,temp_int5,temp_int6,temp_int7;
    for(int i=0; i<num-1; i++)
    {
      for(int j=i+1; j<num; j++)
      {
        if(Array[i][0] > Array[j][0])
        {
          temp_float = Array[i][0];
          temp_int1 = Array[i][1];
		  temp_int2 = Array[i][2];
		  temp_int3 = Array[i][3];
		  temp_int4 = Array[i][4];
		  temp_int5 = Array[i][5];
		  temp_int6 = Array[i][6];

          Array[i][0] = Array[j][0];
          Array[i][1] = Array[j][1];
		  Array[i][2] = Array[j][2];
          Array[i][3] = Array[j][3];
          Array[i][4] = Array[j][4];
		  Array[i][5] = Array[j][5];
		  Array[i][6] = Array[j][6];

          Array[j][0] = temp_float;
		  Array[j][1] = temp_int1;
		  Array[j][2] = temp_int2;
		  Array[j][3] = temp_int3;
		  Array[j][4] = temp_int4;
		  Array[j][5] = temp_int5;
		  Array[j][6] = temp_int6;		  
        }
      }
    }
}

//任意角度转换到0-360
double Angles_PI(double angle)
{
	if (angle<0.0) 
	{
		while (1) 
		{
			angle+=360.0;
			if (angle>=0.0) break;
		}
	}

	else if (angle>=360.0) 
	{
		while (1) 
		{
			angle-=360.0;
			if (angle<360.0) break;
 		}
	}
	return angle;
}

